#include <iostream>
#include <random>
#include <vector>
#include <time.h>
#include <Eigen/SVD>


void LineFitting()
{
    //假设随机三个点
    Eigen::Vector2d p1{1,1};
    Eigen::Vector2d p2{2,2};
    Eigen::Vector2d p3{3,2};

    //Ax+By+C = 0；
    //1.去中心化
    Eigen::Vector2d center = (p1+p2+p3)/3.f;
    p1 = p1 - center;
    p2 = p2 - center;
    p3 = p3 - center;
    
    Eigen::MatrixXd A(3,2);
    A.row(0) = p1.transpose();
    A.row(1) = p2.transpose();
    A.row(2) = p3.transpose();
    std::cout << "A:" << std::endl << A << std::endl;
    // A(0,1) = -0.667;
    // A(1,1) = 0.333;
    // A(2,1) = 0.333;
    // std::cout << "A:" << std::endl << A << std::endl;

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A,Eigen::ComputeThinU | Eigen::ComputeThinV);
    std::cout<<"U:\n"<<svd.matrixU()<<std::endl;
    std::cout<<"V:\n"<<svd.matrixV()<<std::endl;
    std::cout<<"S:\n"<<svd.singularValues()<<std::endl; 
    Eigen::Vector2d  ab = svd.matrixV().col(1);
    std::cout << "ab:" << std::endl << ab << std::endl;
    double D = -ab.x()*center.x() - ab.y()*center.y();
    std::cout<<"D:"<<D<<std::endl;

}



int main()
{
    LineFitting();

    return 0;
}